function u = policy1(weight, x, x_des, idx)
%POLICY1 Summary of this function goes here
%   Detailed explanation goes here

rpy = q_to_euler(x(idx.q));
rpy_des = q_to_euler(x_des(idx.q));
error_pos = express_vector_in_quat_frame(x(idx.ned,:) - x_des(idx.ned,:), x(idx.q));
error_vel = express_vector_in_quat_frame(x(idx.ned_dot,:) - x_des(idx.ned_dot,:), x(idx.q));
l1 = weight(1:5)*[1; tanh(error_pos(1)); error_pos(1); error_vel(1); rpy(2)];
l2 = weight(6:9)*[tanh(error_pos(2)); error_pos(2); error_vel(2); rpy(1)];
l3 = weight(10:12)*[tanh(error_pos(3)); error_pos(3); error_vel(3)];
l4 = weight(13:14)*[x(idx.pqr(3),:); wrapToPi(rpy(3) - rpy_des(3))];


u_unarranged = [weight(15:16)*[tanh(l1); l1]; weight(17:18)*[tanh(l2); l2]; weight(19:20)*[tanh(l3);l3]; weight(21:22)*[tanh(l4);l4]]; %;long, lat, collective, rudder

u = [u_unarranged(2); u_unarranged(1); u_unarranged(4); u_unarranged(3)];
end

